﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows.Media.Imaging;
using Microsoft.Research.Kinect.Nui;
using System.Reactive.Subjects;
using System.Windows.Media;
using System.Reactive.Linq;

namespace Kinect.KinectMouseControl
{
    public class DepthFrameToBitmapSourceConverter : IEventPublisher<BitmapSource>
    {
        IObservable<BitmapSource> IEventPublisher<BitmapSource>.EventObservable { get { return _bitmapImageSubject; } }
        private Subject<BitmapSource> _bitmapImageSubject;

        private IEventPublisher<DepthFrame> _imageFramePublisher;
        private int _frameRateMilliseconds;

        public DepthFrameToBitmapSourceConverter(IEventPublisher<DepthFrame> imageFramePublisher, int frameRateMilliseconds)
        {
            _imageFramePublisher = imageFramePublisher;
            _frameRateMilliseconds = frameRateMilliseconds;
            SetupBitmapImageObserver();
        }

        private void SetupBitmapImageObserver()
        {
            _bitmapImageSubject = new Subject<BitmapSource>();
            _imageFramePublisher
                .EventObservable
                .Sample(TimeSpan.FromMilliseconds(_frameRateMilliseconds))
                .ObserveOnDispatcher()
                .Subscribe(x => ConvertImageToBitmap(x.UnderlyingFrame));
        }

        public void ConvertImageToBitmap(ImageFrame imageFrame)
        {
            PlanarImage imageData = imageFrame.Image;

            byte[] convertedDepthFrame = convertDepthFrame(imageData.Bits);

            var bitmapSource = BitmapSource.Create(imageData.Width, imageData.Height, 96, 96,
                 PixelFormats.Bgr32, null, convertedDepthFrame, imageData.Width * 4);
            _bitmapImageSubject.OnNext(bitmapSource);
        }

        // Code taken from Microsoft's Kinect sample

        const int RED_IDX = 2;
        const int GREEN_IDX = 1;
        const int BLUE_IDX = 0;

        byte[] depthFrame32 = new byte[320 * 240 * 4];

        byte[] convertDepthFrame(byte[] depthFrame16)
        {
            for (int i16 = 0, i32 = 0; i16 < depthFrame16.Length && i32 < depthFrame32.Length; i16 += 2, i32 += 4)
            {
                int player = depthFrame16[i16] & 0x07;
                int realDepth = (depthFrame16[i16 + 1] << 5) | (depthFrame16[i16] >> 3);
                // transform 13-bit depth information into an 8-bit intensity appropriate
                // for display (we disregard information in most significant bit)
                byte intensity = (byte)(255 - (255 * realDepth / 0x0fff));

                depthFrame32[i32 + RED_IDX] = 0;
                depthFrame32[i32 + GREEN_IDX] = 0;
                depthFrame32[i32 + BLUE_IDX] = 0;

                // choose different display colors based on player
                switch (player)
                {
                    case 0:
                        depthFrame32[i32 + RED_IDX] = (byte)(intensity / 2);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(intensity / 2);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(intensity / 2);
                        break;
                    case 1:
                        depthFrame32[i32 + RED_IDX] = intensity;
                        break;
                    case 2:
                        depthFrame32[i32 + GREEN_IDX] = intensity;
                        break;
                    case 3:
                        depthFrame32[i32 + RED_IDX] = (byte)(intensity / 4);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(intensity);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(intensity);
                        break;
                    case 4:
                        depthFrame32[i32 + RED_IDX] = (byte)(intensity);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(intensity);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(intensity / 4);
                        break;
                    case 5:
                        depthFrame32[i32 + RED_IDX] = (byte)(intensity);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(intensity / 4);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(intensity);
                        break;
                    case 6:
                        depthFrame32[i32 + RED_IDX] = (byte)(intensity / 2);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(intensity / 2);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(intensity);
                        break;
                    case 7:
                        depthFrame32[i32 + RED_IDX] = (byte)(255 - intensity);
                        depthFrame32[i32 + GREEN_IDX] = (byte)(255 - intensity);
                        depthFrame32[i32 + BLUE_IDX] = (byte)(255 - intensity);
                        break;
                }
            }
        
            return depthFrame32;
        }
    }
}
